import serial
import struct
import time
import logging
import sys
from serial.tools import list_ports

# 配置日志
logging.basicConfig(
    level=logging.DEBUG,
    format='%(asctime)s - %(name)s - %(levelname)s - %(message)s',
)
logger = logging.getLogger(__name__)


def crc8(datagram):
    """计算CRC8校验值，多项式0x07，初始值0x00"""
    crc = 0
    for byte in datagram:
        current_byte = byte
        for _ in range(8):
            bit = (crc >> 7) ^ (current_byte & 0x01)
            if bit:
                crc = (crc << 1) ^ 0x07
            else:
                crc = (crc << 1)
            crc &= 0xFF  # 保持8位
            current_byte >>= 1
    return crc

def build_command_frame(motor_count, ids, command_types, data_list):
    """
    构造主机到电机控制器的数据帧
    :param motor_count: 电机数量（1-8）
    :param ids: 每个电机的ID列表，长度等于motor_count
    :param command_types: 每个电机的指令类型列表
    :param data_list: 每个电机的数据列表，元素为bytes类型
    :return: 完整的字节数据帧
    """
    if motor_count < 1 or motor_count > 8:
        raise ValueError("电机数量必须在1-8之间")
    if len(ids) != motor_count or len(command_types) != motor_count or len(data_list) != motor_count:
        raise ValueError("参数长度与电机数量不匹配")
    
    # 构造所有指令块
    instruction_blocks = []
    for i in range(motor_count):
        id_byte = bytes([ids[i]])
        cmd_byte = bytes([command_types[i]])
        data_bytes = data_list[i]
        block = id_byte + cmd_byte + data_bytes
        instruction_blocks.append(block)
    
    # 合并指令块
    instruction_data = b''.join(instruction_blocks)
    
    # 计算数据长度字段：2(FF FF) + 1（自身） + 1（电机数量） + 指令块总长度 + 1（CRC）
    data_length = 2 + 1 + 1 + len(instruction_data) + 1
    if data_length > 128:
        raise ValueError("数据长度超过最大限制128字节")
        
    # 构造完整数据帧
    frame = b'\xff\xff'  # 字头
    frame += bytes([data_length])
    frame += bytes([motor_count])
    frame += instruction_data
    crc_value = crc8(frame)
    frame += bytes([crc_value])
    
    return frame

def receive_response(ser, expected_length=14, timeout=1):
    """接收响应数据，并校验结构"""
    start_time = time.time()
    buffer = bytearray()
    while time.time() - start_time < timeout:
        if ser.in_waiting > 0:
            buffer += ser.read(ser.in_waiting)
            # 查找字头0xFF 0xFF
            while len(buffer) >= 2:
                pos = buffer.find(b'\xff\xff')
                if pos == -1:
                    # 没有找到字头，保留最后一个字节继续查找
                    buffer = buffer[-1:] if buffer else bytearray()
                    break
                else:
                    # 找到字头，截取后续数据
                    buffer = buffer[pos:]
                    if len(buffer) < expected_length:
                        # 数据不足，继续等待
                        break
                    else:
                        # 提取完整帧
                        frame = buffer[:expected_length]
                        buffer = buffer[expected_length:]
                        return frame
        time.sleep(0.01)
    return None

def parse_stat_response(frame):
    """解析查询指令的响应数据"""
    if len(frame) != 14:
        raise ValueError("响应帧长度必须为14字节")
    # 校验字头
    if frame[0] != 0xFF or frame[1] != 0xFF:
        raise ValueError("无效的字头")
    # 提取数据部分和CRC
    data_part = frame[0:13]
    received_crc = frame[13]
    # 计算CRC
    calculated_crc = crc8(data_part)
    if calculated_crc != received_crc:
        raise ValueError(f"CRC校验失败: 计算值{calculated_crc:02X}, 接收值{received_crc:02X}")
    # 解析数据字段
    cmd_count = struct.unpack('<H', data_part[3:5])[0]
    trap_status = data_part[5]
    temperature = struct.unpack('b', data_part[6:7])[0]
    pos = struct.unpack('<i', data_part[7:11])[0]
    voltage = struct.unpack('<h', data_part[11:13])[0]
    return [cmd_count, trap_status, temperature, pos, voltage]

# 使能、禁用编号为id_list的电机，可同步控制多个
# 用法举例，禁用编号为1,2的电机：cmd_enable(ser,[1,2],0)
def cmd_enable(ser, id_list, en):
    # 构造使能指令（0x01）
    data = bytes([en])  # 启用动力
    motor_count = len(id_list) 
    enable_frame = build_command_frame(
        motor_count, 
        id_list, 
        [0x01] * motor_count, 
        [data] * motor_count)
    logger.debug(f"使能指令数据帧: {enable_frame.hex()}")
    ser.write(enable_frame)

    # 接收响应
    response_frame = receive_response(ser, 4)

    if response_frame:
        logger.debug(f"接收到响应帧: {response_frame.hex()}")
        parsed_data = (response_frame)
        # 计算CRC
        calculated_crc = crc8(response_frame[0:3])
        received_crc = response_frame[3]
        if calculated_crc != received_crc:
            raise ValueError(f"CRC校验失败: 计算值{calculated_crc:02X}, 接收值{received_crc:02X}")
        return True
    else:
        logger.debug("未接收到响应")
        return False


def cmd_stat(ser, id):
    # 构造查询指令（0x00）
    motor_count = 1
    ids = [id]
    command_types = [0x00]
    data_list = [bytes()]  # 无数据部分
    stat_frame = build_command_frame(motor_count, ids, command_types, data_list)
    logger.debug(f"电机编号: {id}, 查询指令数据帧: {stat_frame.hex()}")
    ser.write(stat_frame)

    # 接收并解析响应
    response_frame = receive_response(ser, 14)
    if response_frame:
        logger.debug(f"接收到响应帧: {response_frame.hex()}")
        parsed_data = parse_stat_response(response_frame)
        logger.debug(f"[count, trap, temp, pos, voltage]={parsed_data}")
        return parsed_data
    else:
        logger.debug("未接收到响应")
        return []

def list_serial_ports():
    """列出可用串口"""
    ports = list_ports.comports()
    if not ports:
        print("未检测到可用串口设备")
        return
    print("可用串口设备:")
    for port in ports:
        print(f"  {port.device} - {port.description}")


def test_cmd_enable(ser):
    print("\n--- 测试使能指令 ---")
    id_list = list(map(int, input("请输入电机ID列表(空格分隔): ").split()))
    en = int(input("使能状态(0/1): "))
    success = cmd_enable(ser, id_list, en)
    print("执行结果:", "成功" if success else "失败")

def test_cmd_stat(ser):
    print("\n--- 测试状态查询 ---")
    motor_id = int(input("请输入要查询的电机ID: "))
    result = cmd_stat(ser, motor_id)
    print("查询结果:", result if result else "无响应")

if __name__ == "__main__":
    if len(sys.argv) > 1:
        serial_port = sys.argv[1]
    else:
        serial_port = '/dev/cu.usbserial-130'  # 默认值

    baud_rate = 1000000  # 1Mbps
    try:
        with serial.Serial(serial_port, baudrate=baud_rate, bytesize=serial.EIGHTBITS,
                        parity=serial.PARITY_NONE, stopbits=serial.STOPBITS_ONE) as ser:
            print("\n串口连接成功，进入交互模式")
            while True:
                print("\n请选择测试项目:")
                print("[1]: 测试使能指令(cmd_enable)")
                print("[2]: 测试状态查询(cmd_stat)")
                print("[q]: 退出程序")
                
                choice = input("请输入选项(1/2/q) >> ").strip().lower()
                
                if choice in ['exit', 'quit', 'q']:
                    print("退出程序...")
                    break
                    
                elif choice == '1':
                    test_cmd_enable(ser)
                    
                elif choice == '2':
                    test_cmd_stat(ser)
                    
                else:
                    print("无效选项，请重新输入")
                    
                # 添加短暂等待让用户看到结果
                time.sleep(0.5)

            # while(True):
            #     pos = [0,0,0,0]
            #     for id in (1,2,3,4):
            #         resp = cmd_stat(ser, id)
            #         pos[id-1] = resp[3]
            #         time.sleep(0.1)
            #     print(f"当前电机位置: {pos}")

            # resp = cmd_stat(ser, 2)
            # logger.info(resp)
            
            # # 构造梯形运动指令（0x02）
            # x1 = 0  # 目标位置
            # v1 = 0      # 目标速度
            # vmax = 500  # 最大速度
            # a = 100     # 加速度
            # max_current = 35 # 电流限制35%

            # # 小端序打包数据
            # data_bytes = struct.pack('<i', x1)   # int32
            # data_bytes += struct.pack('<h', v1)  # int16
            # data_bytes += struct.pack('<h', vmax)# int16
            # data_bytes += struct.pack('<h', a)   # int16
            # data_bytes += bytes([max_current])   # uint8

            # trap_frame = build_command_frame(1, [0x02], [0x02], [data_bytes])
            # print(f"梯形运动指令数据帧: {trap_frame.hex()}")
            # ser.write(trap_frame)
            # time.sleep(0.2)
    except serial.SerialException as e:
        logger.error(f"打开串口 {serial_port} 失败: {str(e)}")
        list_serial_ports()
        sys.exit(1)
    except KeyboardInterrupt:
        logger.info("程序被用户中断")

